from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration

def main():
    rclpy.init()
    navigator = BasicNavigator()
    # 等待导航启动完成
    navigator.waitUntilNav2Active()
    # set goal
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 6.5
    goal_pose.pose.position.y = 3.0
    goal_pose.pose.orientation.w = 1.0
    # send goal & receive feedback
    navigator.goToPose(goal_pose)
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f"predict after:{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds/1e9}s finish")
        # over time -> cancel
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelTask()
    # judge final result
    result = navigator.getResult()
    if result==TaskResult.SUCCEEDED:
        navigator.get_logger().info('navigation 001 result: success')
    elif result==TaskResult.CANCELED:
        navigator.get_logger().info('navigation 001 result: canceled')
    elif result==TaskResult.FAILED:
        navigator.get_logger().info('navigation 001 result: failed')
    else:
        navigator.get_logger().info('navigation 001 result: return invalid')
        
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 5.0
    goal_pose.pose.position.y = 0.0
    goal_pose.pose.orientation.w = 1.0
    # send goal & receive feedback
    navigator.goToPose(goal_pose)
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f"predict after:{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds/1e9}s finish")
        # over time -> cancel
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelTask()
    # judge final result
    result = navigator.getResult()
    if result==TaskResult.SUCCEEDED:
        navigator.get_logger().info('navigation 002 result: success')
    elif result==TaskResult.CANCELED:
        navigator.get_logger().info('navigation 002 result: canceled')
    elif result==TaskResult.FAILED:
        navigator.get_logger().info('navigation 002 result: failed')
    else:
        navigator.get_logger().info('navigation 002 result: return invalid')